#include <GL/freeglut.h>
#include <opencv2/opencv.hpp>

#include "hvrl/video/OpenNIDepthInputStream.hpp"
#include "hvrl/common/Common.hpp"

#include <glm/gtc/type_ptr.hpp>

using namespace hvrl;

unsigned int createTexture(const cv::Mat& img, const cv::Size& s, const GLenum& type);
void updateTexture(unsigned int& texid, const unsigned short* img, const cv::Size& s, const GLenum& type);

cv::Size windowsize(800,600);

unsigned int frametexid = 0;

float near = 0.01f;
float far = 100.0f;
bool isrunning = true;
int timebase = 0;

OpenNIDepthInputStream *openni = 0;

void drawFullScreenQuad(const unsigned int& width, const unsigned int& height, const unsigned int& textureid)
{

    glPushAttrib(GL_ALL_ATTRIB_BITS);

    glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
    glDisable(GL_DEPTH_TEST);

    glDisable(GL_LIGHTING);

    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    glOrtho(0,width,height,0,-0.1,0.1);
    glViewport(0,0,width,height);

    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();

    glBindTexture(GL_TEXTURE_2D,textureid);

    glBegin(GL_QUADS);
    glColor3f(1.0f,1.0f,1.0f);
    glTexCoord2f(0.0f,0.0f);
    glVertex2i(0,0);
    glTexCoord2f(1.0f,0.0f);
    glVertex2i(width,0);
    glTexCoord2f(1.0f,1.0f);
    glVertex2i(width,height);
    glTexCoord2f(0.0f,1.0f);
    glVertex2i(0,height);
    glEnd();

    glBindTexture(GL_TEXTURE_2D,0);

    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
    glPopMatrix();

    glPopAttrib();
}


static void displayGL(void)
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glPushMatrix();

    cv::Size ds = openni->getDepthSize();
    const unsigned short * tmp = openni->getDepth();
    updateTexture(frametexid , tmp, ds, GL_LUMINANCE);

    drawFullScreenQuad(windowsize.width, windowsize.height, frametexid);

#ifdef DEBUG
    GLenum err = glGetError();
    if(err!=GL_NO_ERROR)
    {
        std::cerr << "Une erreur s'est produite : "  << std::endl;
        std::cerr << gluErrorString(err) << std::endl;
    }
#endif

    glScalef(1.0,-1.0,-1.0);
    glPointSize(1.5f);
    const unsigned char * tmp_color = openni->getColor();
    const float bpc = openni->getBackProjection();

    // Convert into a 3D map
    glBegin(GL_POINTS);
    for(int i = 0; i < ds.height; ++i)
    {
        for(int j = 0; j < ds.width; ++j)
        {
            float z = tmp[i*ds.width+j] * 0.001f;
            float x = (int(j) - ds.width/2)*z*bpc;
            float y = (int(i) - ds.height/2)*z*bpc;
            glColor3ubv(&(tmp_color[(i*ds.width+j)*3]));
            glVertex3f(x,y,z);
        }
    }
    glEnd();
    glPopMatrix();

    glutSwapBuffers();

}

static void reshapeGL(int newwidth,
                      int newheight)
{
    float ratio = (float)newwidth/(float)newheight;
    glViewport(0, 0, (GLint)newwidth, (GLint)newheight);

    windowsize.width = newwidth;
    windowsize.height = newheight;

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0f, ratio, 0.1f, 100.0f);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

}

unsigned int createTexture(const unsigned short* img, const cv::Size& s, const GLenum& type)
{

    GLuint texid = 0;
    glGenTextures(1,&texid);
    if(texid==0)
    {
        // error("createTexture","unable to create a texture ID");
        return 0;
    }

    glBindTexture(GL_TEXTURE_2D,texid);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glTexImage2D (GL_TEXTURE_2D,0,GL_RGB,s.width,s.height,0,type,GL_UNSIGNED_SHORT,img);
    glBindTexture(GL_TEXTURE_2D,0);
    return texid;

}

void updateTexture(unsigned int& texid, const unsigned short* img, const cv::Size& s,const GLenum& type)
{

    if(texid == 0)
    {
        texid = createTexture(img,s,type);
    }
    else
    {
        GLint width,height;
        glGetTexLevelParameteriv(GL_TEXTURE_2D,0,GL_TEXTURE_WIDTH,&width);
        glGetTexLevelParameteriv(GL_TEXTURE_2D,0,GL_TEXTURE_HEIGHT,&height);
        if(s.width!= width || s.height!=height)
        {
            glDeleteTextures(1,&texid);
            texid = createTexture(img,s,type);
        }
        else
        {
            glBindTexture(GL_TEXTURE_2D,texid);
            glTexSubImage2D(GL_TEXTURE_2D,0,GL_RGB,s.width,s.height,0,type,GL_UNSIGNED_SHORT,img);
            glBindTexture(GL_TEXTURE_2D,0);
        }

    }
}

static void idleGL(void)
{
    int dt = glutGet(GLUT_ELAPSED_TIME) - timebase;
    if(dt > 30)
    {
        openni->update();

        //cv::Mat img = openni->getDepthFrame();

        glutPostRedisplay();
        timebase += dt;

    }
}

static void quit(void)
{
    glDeleteTextures(1,&frametexid);
    isrunning = false;
}

static void keyboardGL(unsigned char c, int x, int y)
{
    switch(c)
    {
    case 27:
    case 'q':
        quit();
        break;
    case 's':
    {
      /*  cv::Mat img = openni->getColorFrame();
        cv::imwrite("color.png",img);
        img = openni->getDepthFrame();
        cv::imwrite("depth.png",img);*/
    }
        break;
    default:
        break;
    }
    glutPostRedisplay();
}

static bool initGL(int argc,
                   char **argv)
{

    glClearColor(0.8f,0.8f,0.8f,1.0f);

    glDisable(GL_LIGHTING);
    glDisable(GL_LIGHT0);

    glEnable(GL_DEPTH_TEST);

    //glEnable(GL_CULL_FACE);

    glShadeModel(GL_SMOOTH);

    glEnable(GL_TEXTURE_2D);

    glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);

    glPixelStorei(GL_UNPACK_ALIGNMENT,1);
    glPixelStorei(GL_PACK_ALIGNMENT,1);

try{
    openni = new OpenNIDepthInputStream();
}catch(Exception& e)
{
    std::cout << e << std::endl;
    quit();
}

    return true;
}

int main(int argc, char **argv)
{
    unsigned int windowid = 0;
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);

    glutInitWindowPosition(0, 0);
    glutInitWindowSize(windowsize.width, windowsize.height);


    if((windowid=glutCreateWindow(" ")) == 0)
        return 1;

    if(!initGL(argc, argv))
        quit();

    glutReshapeFunc(reshapeGL);
    glutDisplayFunc(displayGL);
    glutIdleFunc(idleGL);
    glutKeyboardFunc(keyboardGL);

//glutMainLoop();
    while(isrunning)
    {
        idleGL();
        glutMainLoopEvent();

    }

    glutDestroyWindow(windowid);
    return 0;
}

